#include <linux/head.h>
#include <linux/kernel.h>
#include <linux/ptrace.h>

#include <asm/system.h>
#include <asm/io.h>

#define DO_ERROR(trapnr, str, name)                         \
    void do_##name(struct pt_regs *regs, long error_code) { \
        die_if_kernel(str, regs, error_code);               \
    }

#define get_seg_byte(seg, addr) ({ \
register char __res; \
__asm__("push %%fs\n\tmov %%ax,%%fs\n\tmovb %%fs:%2,%%al\n\tpop %%fs" \
    :"=a" (__res):"0" (seg),"m" (*(addr))); \
__res; })

#define get_seg_long(seg, addr) ({ \
register unsigned long __res; \
__asm__("push %%fs\n\tmov %%ax,%%fs\n\tmovl %%fs:%2,%%eax\n\tpop %%fs" \
    :"=a" (__res):"0" (seg),"m" (*(addr))); \
__res; })

void divide_error(void);
void debug(void);
void nmi(void);
void int3(void);
void overflow(void);
void bounds(void);
void invalid_op(void);
void double_fault(void);
void coprocessor_segment_overrun(void);
void invalid_TSS(void);
void segment_not_present(void);
void stack_segment(void);
void general_protection(void);
void reserved(void);
void alignment_check(void);

void die_if_kernel(char *str, struct pt_regs *regs, long err) {
    int i;
    unsigned long esp;
    unsigned short ss;

    esp = (unsigned long)&regs->esp;
    ss = KERNEL_DS;
    if (regs->cs & 3) {
        esp = regs->esp;
        ss = regs->ss;
    }
    printk("%s: %04lx\n", str, err & 0xffff);
    printk("EIP:    %04x:%08lx\nEFLAGS: %08lx\n", 0xffff & regs->cs, regs->eip, regs->eflags);
    printk("eax: %08lx   ebx: %08lx   ecx: %08lx   edx: %08lx\n",
           regs->eax, regs->ebx, regs->ecx, regs->edx);
    printk("esi: %08lx   edi: %08lx   ebp: %08lx   esp: %08lx\n",
           regs->esi, regs->edi, regs->ebp, esp);
    printk("ds: %04x   es: %04x   fs: %04x   gs: %04x   ss: %04x\n",
           regs->ds, regs->es, regs->fs, regs->gs, ss);
    for (i = 0; i < 5; i++)
        printk("%08lx ", get_seg_long(ss, (i + (unsigned long *)esp)));
    printk("\nCode: ");
    for (i = 0; i < 20; i++)
        printk("%02x ", 0xff & get_seg_byte(regs->cs, (i + (char *)regs->eip)));
    printk("\n");
    while (1) {}
}

DO_ERROR(0, "divide error", divide_error)
DO_ERROR(3, "int3", int3)
DO_ERROR(4, "overflow", overflow)
DO_ERROR(5, "bounds", bounds)
DO_ERROR(6, "invalid operand", invalid_op)
DO_ERROR(8, "double fault", double_fault)
DO_ERROR(9, "coprocessor segment overrun", coprocessor_segment_overrun)
DO_ERROR(10, "invalid TSS", invalid_TSS)
DO_ERROR(11, "segment not present", segment_not_present)
DO_ERROR(12, "stack segment", stack_segment)
DO_ERROR(13, "general protection", general_protection)
DO_ERROR(15, "reserved", reserved)
DO_ERROR(17, "alignment check", alignment_check)

void do_nmi(struct pt_regs *regs, long error_code) {
    printk("Uhhuh. NMI received. Dazed and confused, but trying to continue\n");
    printk("You probably have a hardware problem with your RAM chips\n");
}

void do_debug(struct pt_regs *regs, long error_code) {
    if ((regs->cs & 3) == 0) {
        /* If this is a kernel mode trap, then reset db7 and allow us to continue */
        __asm__("movl $0, %%edx\n\t"
                "movl %%edx, %%db7\n\t"
                : /* no output */
                : /* no input */
                : "dx");
        return;
    };
    die_if_kernel("debug", regs, error_code);
}

void pic_init() {
    outb_p(0x11, 0x20); // 边沿触发，多片8259A芯片级联
    outb_p(0x11, 0xa0); // 边沿触发，多片8259A芯片级联
    outb_p(0x20, 0x21); // 主片中断请求0~7对应的中断号是0x20~0x27
    outb_p(0x28, 0xa1); // 从片中断请求8~15级对应的中断号是0x28~0x2f
    outb_p(0x04, 0x21); // 主片的IR2连接一个从片
    outb_p(0x02, 0xa1); // 从片连接到主片的IR2引脚
    outb_p(0x01, 0x21); // 普通全嵌套方式、非缓冲方式、非自动结束中断方式
    outb_p(0x01, 0xa1); // 普通全嵌套方式、非缓冲方式、非自动结束中断方式
    outb_p(0xff, 0x21); // 屏蔽所有中断
    outb_p(0xff, 0xa1); // 屏蔽所有中断
}

void trap_init(void) {
    int i;
    pic_init();
    set_trap_gate(0, &divide_error);
    set_trap_gate(1, &debug);
    set_trap_gate(2, &nmi);
    set_system_gate(3, &int3); /* int3-5 can be called from all */
    set_system_gate(4, &overflow);
    set_system_gate(5, &bounds);
    set_trap_gate(6, &invalid_op);
    set_trap_gate(8, &double_fault);
    set_trap_gate(9, &coprocessor_segment_overrun);
    set_trap_gate(10, &invalid_TSS);
    set_trap_gate(11, &segment_not_present);
    set_trap_gate(12, &stack_segment);
    set_trap_gate(13, &general_protection);
    set_trap_gate(15, &reserved);
    set_trap_gate(17, &alignment_check);
    for (i = 18; i < 48; i++)
        set_trap_gate(i, &reserved);
}
